/* ######################################################################### */
/** \file UpdateReference.hpp
 * \brief	This file contains the function that
 *
 *
 * PROJECT:   DATMO ROS NODE
 *
 * Copyright (c) 2011 CAR - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2011-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ         2011-06-23 \n
 *      First .
 *
 ######################################################################### */


#include <math.h>
/* ------------------------------------------------------------------------- */
/**	\namespace DATMO
 *
 *  \brief	This is the namespace of the DATMO ROS NODE
 */
/* ------------------------------------------------------------------------- */

namespace DATMO
{
/* ------------------------------------------------------------------------- */
/**	\class UpdateReference
 *
 *  \brief  This is the class of UpdateReference function.
 *
 *
 */
/* ------------------------------------------------------------------------- */
class cl_UpdateReference
{
public:
	/* ------------------------------------------------------------------------- */
	/**	\fn public static void UpdateReference()
	 *
	 *  \brief
	 *
	 *  \param[in]
	 *  \param[in]
	 *
	 *  \param[out] bool
	 *
	 */
	/* ------------------------------------------------------------------------- */
	static BackgroundReference UpdateReference(BackgroundReference str_BackgroundRef,
										DistanceImage str_DistImage,
										DistanceImage str_LastDistImage,
										DistanceImage str_LastLastDistImage,
										Matrix D_Obj)
	{
		/*Variables*/
		ROS_INFO("Entra en Update reference");
		float mean_D;
		float var_D;
		for (int i = 1; i <= str_DistImage.s_height; i++)
		{
			for (int j = 1; j <= str_DistImage.s_width; j++)
			{
				if (D_Obj(i, j) == 0)
				{
					mean_D = (str_DistImage.tf_Distance(i, j)
							+ str_LastDistImage.tf_Distance(i, j)
							+ str_LastLastDistImage.tf_Distance(i, j)) / 3;
					var_D =  (pow( (str_DistImage.tf_Distance(i, j)         - mean_D), 2.0) +
							  pow( (str_LastDistImage.tf_Distance(i, j)     - mean_D), 2.0) +
							  pow( (str_LastLastDistImage.tf_Distance(i, j) - mean_D), 2.0));
					//ROS_INFO("valores D: 1) %f 2) %f 3) %f",str_DistImage.tf_Distance(i, j),str_LastDistImage.tf_Distance(i, j),str_LastLastDistImage.tf_Distance(i, j));
					if (var_D < str_BackgroundRef.tf_Vref(i, j))
					{
					//  ROS_INFO("varianza menor antes: %f despues %f",str_BackgroundRef.tf_Vref(i, j),var_D);
						str_BackgroundRef.tf_Vref(i, j) = var_D;
						str_BackgroundRef.tf_Dref(i, j) = mean_D;
					}
					else if (var_D < 10 * str_BackgroundRef.tf_Vref(i, j) && mean_D > str_BackgroundRef.tf_Dref(i, j))
					{
						str_BackgroundRef.tf_Vref(i, j) = var_D;
						str_BackgroundRef.tf_Dref(i, j) = mean_D;
					}
//					else if(str_BackgroundRef.tf_Vref(i, j)==0)
//					{
//					  //ROS_INFO("varianza zero");
//					  str_BackgroundRef.tf_Vref(i, j) = var_D;
//					  str_BackgroundRef.tf_Dref(i, j) = mean_D;
//					}
				}
			}
		}
		return str_BackgroundRef;
	}

};
}

